#include "stanley_node.h"
#include <cmath>
#include <limits>
#include <climits>
#include "common_tool/geometry.hpp"
StartStanleyNs constexpr double max_steering_wheel_angle = 810;
constexpr double STEERING_RATIO = 18;
StanleyNode::StanleyNode(double loop_rate) : loop_rate_(loop_rate),
                                             speed_controller_(0.5, 0.5, 0, 1 / loop_rate, -1, 1, -1, 1)
{
    localizition_dog_.setAllParameter(8, 1, 0, 3, 8);
    chassis_dog_.setAllParameter(8, 1, 0, 3, 8);
    planning_dog_.setAllParameter(8, 1, 0, 17, 25);
}
void StanleyNode::CBLocalizition(const auto_msgs::localizition &msg)
{
    localizition_msg_ = msg;
    localizition_dog_.giveFood();
}
void StanleyNode::CBChassis(const auto_msgs::chassis &msg)
{
    chassis_msg_ = msg;
    chassis_dog_.giveFood();
}
void StanleyNode::CBPlanning(const auto_msgs::planning &msg)
{
    planning_msg_ = msg;
    planning_dog_.giveFood();
}
bool StanleyNode::SearchRefPoint(const auto_msgs::planning &msg)
{
    if (msg.path.size() <= 0)
    {
        ROS_ERROR("StanleyNode::SearchRefPoint failed due to path size is zero");
        return false;
    }
    else if (msg.path.size() == 1)
    {
        lat_controller_.ref_pose_.x = msg.path[0].x;
        lat_controller_.ref_pose_.y = msg.path[0].y;
        lat_controller_.ref_pose_.theta = msg.path[0].theta; //有待改善
        return true;
    }
    else
    {
        double x_front_axle = localizition_msg_.x + lat_controller_.wheelBase() *
                                                        std::cos(localizition_msg_.heading * deg_to_rad);
        double y_front_axle = localizition_msg_.y + lat_controller_.wheelBase() *
                                                        std::sin(localizition_msg_.heading * deg_to_rad);

        //planning msg closet point is based on rear axle,re-search must be done
        double delta_x_debug = 0;
        double delta_y_debug = 0;
        double x_closet_ref = 0;
        double y_closet_ref = 0;
        int closet_point_front_axle = 0;
        double last_distance = std::numeric_limits<double>::max();
        for (int i = 0; i < msg.path.size() - 1; i++) //search closet point to front axle
        {

            double path_x_front_axle = msg.path[i].x + lat_controller_.wheelBase() *
                                                           std::cos(msg.path[i].theta * deg_to_rad);
            double path_y_front_axle = msg.path[i].y + lat_controller_.wheelBase() *
                                                           std::sin(msg.path[i].theta * deg_to_rad);
            double delta_x = path_x_front_axle - x_front_axle;
            double delta_y = path_y_front_axle - y_front_axle;
            double distance = std::pow(delta_x * delta_x + delta_y * delta_y, 0.5);
            if (distance < last_distance)
            {
                x_closet_ref = path_x_front_axle;
                y_closet_ref = path_y_front_axle;
                delta_y_debug = delta_y;
                delta_x_debug = delta_x;
                closet_point_front_axle = i;
                last_distance = distance;
            }
        }
        std::cout << "closet_point_front_axle is " << closet_point_front_axle << std::endl;
        std::cout << "last_distace is " << last_distance << std::endl;
        std::cout << "delta x delta y are " << delta_x_debug << " " << delta_y_debug << std::endl;
        lat_controller_.ref_pose_.x = x_closet_ref;
        lat_controller_.ref_pose_.y = y_closet_ref;
        lat_controller_.ref_pose_.theta = msg.path[closet_point_front_axle].theta; //有待改善
        return true;
    }
}
bool StanleyNode::CheckDog()
{
    bool localizition_flag = false;
    bool chassis_flag = false;
    bool planning_flag = false;
    if (localizition_dog_.isDied())
    {
        localizition_flag = true;
        ROS_ERROR("localizition_dog time out");
    }
    if (chassis_dog_.isDied())
    {
        chassis_flag = true;
        ROS_ERROR("chassis_dog time out");
    }
    if (planning_dog_.isDied())
    {
        planning_flag = true;
        ROS_ERROR("planning_dog time out");
    }
    if (localizition_flag || chassis_flag || planning_flag)
    {
        return false;
    }
    return true;
}
void StanleyNode::NoFood()
{
    localizition_dog_.noFood();
    chassis_dog_.noFood();
    planning_dog_.noFood();
}

void StanleyNode::HardBrake()
{
    control_cmd_.brake = 1;
    control_cmd_.throttle = 0;
    speed_controller_.resetIntergration();
}
void StanleyNode::UpdateStanleyLateralInput()
{
    lat_controller_.current_pose_.x = localizition_msg_.x;
    lat_controller_.current_pose_.y = localizition_msg_.y;
    lat_controller_.current_pose_.theta = localizition_msg_.heading;
    lat_controller_.speed_mps_ = std::abs(chassis_msg_.speed_mps);
}
void StanleyNode::UpdateStanleyDebugMsg()
{
    stanley_debug_msg_.delta_x_g_ = lat_controller_.delta_x_g_;
    stanley_debug_msg_.delta_y_g_ = lat_controller_.delta_y_g_;
    stanley_debug_msg_.delta_theta_ = lat_controller_.delta_theta_;
    stanley_debug_msg_.lateral_error_ = lat_controller_.lateral_error_;
    stanley_debug_msg_.longtitude_error_ = lat_controller_.longtitude_error_;
}
bool StanleyNode::LateralCtr()
{
    bool temp_flag = lat_controller_.step();
    control_cmd_.steering_target = lat_controller_.tire_angle_ * STEERING_RATIO / max_steering_wheel_angle;
    return temp_flag;
}
bool StanleyNode::LongtitudeCtr()
{
    double current_speed = std::abs(chassis_msg_.speed_mps);
    double speed_cmd = planning_msg_.path[planning_msg_.closet_point_num].v / 3.6;
    double speed_error = speed_cmd - current_speed;
    ROS_INFO("speed error is %f ", speed_error);
    double temp_acc = speed_controller_.step(speed_error);
    if (temp_acc >= 0)
    {
        control_cmd_.throttle = std::abs(temp_acc);
        control_cmd_.brake = 0;
    }
    else
    {
        control_cmd_.throttle = 0;
        control_cmd_.brake = std::abs(temp_acc);
    }
    return true;
}

void StanleyNode::LoopProc()
{
    NoFood();

    if (!CheckDog())
    {
        HardBrake();
        control_cmd_pub_.publish(control_cmd_);
        return;
    }
    if (!SearchRefPoint(planning_msg_))
    {
        HardBrake();
        control_cmd_pub_.publish(control_cmd_);
        return;
    }
    UpdateStanleyLateralInput();
    if (!LateralCtr())
    {
        HardBrake();
        control_cmd_pub_.publish(control_cmd_);
        return;
    }
    LongtitudeCtr();
    control_cmd_pub_.publish(control_cmd_);
    UpdateStanleyDebugMsg();
    stanley_debug_pub_.publish(stanley_debug_msg_);
}

EndStanleyNs
